#include "EDLib.h"
#include <iostream>
#include <string.h>
#include <time.h>

// ROS
#include <actionlib/server/simple_action_server.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Header.h>
#include <std_msgs/Int32.h>
#include <custom_msgs/CircleRecog.h>
#include "geometry_msgs/PoseStamped.h"


// OpenCv
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/core/core.hpp> 
#include <opencv2/highgui/highgui.hpp> 
#include<opencv2/imgproc/imgproc.hpp>  

// c++
#include <pthread.h>
#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>
#include <vector>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

//Eigen
#include "Eigen/Core"
#include "Eigen/Dense"

namespace circle_detection{
    class CircleDetection{
        public:
            CircleDetection(ros::NodeHandle nh);
            ~CircleDetection();
        private:
            std::string left_topic;
            std::string right_topic;
            ros::NodeHandle n;
            ros::Subscriber left_sub,right_sub;
            ros::Subscriber pos_sub_;
            Eigen::Vector3d odom_pos_;
            Eigen::Quaterniond odom_orient_;
            ros::Publisher pub_recg;
            image_transport::Publisher pic_pub;
            geometry_msgs::PoseStampedConstPtr pos_;
            cv::Mat left_buf, right_buf;
            cv::Mat depth_buf,disparity;
            const double camera_fx_ = 320;
            const double camera_cx_ = 320;
            const double camera_cy_ = 240;
            Eigen::Matrix3d camera_K_;
            Eigen::Matrix3d camera_K_inv_;
            

            void imageCallback(const sensor_msgs::ImageConstPtr &left_image,const sensor_msgs::ImageConstPtr &right_image);
            void imgdepth_callback(const sensor_msgs::ImageConstPtr &img_msg);
            void init();


            // message filter topics
            message_filters::Subscriber<sensor_msgs::Image> left_sub_, right_sub_;
            typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> mySyncPolicy;
            message_filters::Synchronizer<mySyncPolicy> cam_sync{mySyncPolicy(10)};
            //message_filters::Synchronizer<MySyncPolicy> cam_sync(MySyncPolicy(10), left_sub, right_sub);//创建了一个消息同步器，将左右图像的消息进行同步处理
            void initCamTopics(ros::NodeHandle &nh) {
                nh.param<std::string>("right_topic", right_topic, "/airsim_node/drone_1/front_right/Scene");
                nh.param<std::string>("left_topic", left_topic, "/airsim_node/drone_1/front_left/Scene");
                left_sub_.subscribe(nh, left_topic ,1);
                right_sub_.subscribe(nh, right_topic ,1);
                cam_sync.connectInput(left_sub_, right_sub_);
                cam_sync.registerCallback(boost::bind(&CircleDetection::imageCallback, this, _1, _2));
                //std::cout << "0000000000000000" << std::endl;
               // ros::spin();
            }


            //Image Process
            //cv::Mat getDepthImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg);
            void RemoveFarAwayPoints(cv::Mat src, cv::Mat dst);
            cv::Mat DepthFloat2Int(cv::Mat src);
            cv::Mat ColorFloat2Int(cv::Mat src);
            cv::Mat filterImg(cv::Mat src);
            cv::Mat getDepthImageFromSence(const cv::Mat &disparity);
            cv::Mat computesgbm(const sensor_msgs::ImageConstPtr &left_image,const sensor_msgs::ImageConstPtr &right_image);

            // //for Depth Detection
            void getDepth2Circles(const std::vector<mCircle>&circles);
            void getDepth1Circle1Ellipse(const std::vector<mCircle> &circles, const std::vector<mEllipse> &ellipses);
            void getDepth2Ellipses(const std::vector<mEllipse> &ellipses);

            // //utils
            bool InsideEllipse(const mEllipse &ellipse,float ptx, float pty);
            bool InsideCircle(const mCircle &circle, float ptx, float pty);
            bool OutsideCircle(const mCircle &circle, float ptx, float pty);
            Eigen::Vector3d FromImage2CameraFrame(const Eigen::Vector3d &v1);
            void posCallback(const geometry_msgs::PoseStampedConstPtr &msg);
            void transformCamera2World(const Eigen::Vector3d &pixel_and_depth);



            // Eigen::Vector3d crossProduct(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2);
            //Publisher
            void PublishResult(const Eigen::Vector3d &CenterPt);







    };
}